
#include "loirey_FFT.h"

int loirey::fft::mUnitRoots = -1;
int loirey::fft::nUnitRoots = 0;
CSimpleTypeArray<CComplexNumber> loirey::fft::vUnitRoots;

void loirey::fft::ensure_unit_roots( int M )
{
	if (mUnitRoots >= M) return;
	int i;

	mUnitRoots = M;
	nUnitRoots = (1 << M);
	vUnitRoots.Resize(nUnitRoots);
	for ( i = 0; i < nUnitRoots; ++i )
		vUnitRoots.pElement[i].Init_by_Polar(i*(PI+PI)/nUnitRoots);
}

void loirey::fft::set_complex_unit( int M, int k, CComplexNumber& Target )
{
	k <<= (mUnitRoots - M);
	Target.rel = vUnitRoots.pElement[k].rel;
	Target.img = vUnitRoots.pElement[k].img;
}

bool loirey::fft::fft_Small_M_Only(
	int M, bool fInverse,
	const CComplexNumber* qSrc,
	CComplexNumber* pDst )
{
	if (M < 1)
	{
		*pDst = *qSrc;
		return true;
	}

	if (M == 1)
	{
		pDst[0] = qSrc[0] + qSrc[1]; if (fInverse) pDst[0] /= 2;
		pDst[1] = qSrc[0] - qSrc[1]; if (fInverse) pDst[1] /= 2;
		return true;
	}

	return false;
}

void loirey::fft::fft_Radix2(
	int M, bool fInverse,
	const CComplexNumber* qSrc,
	CComplexNumber* pDst )
{
	if (fft_Small_M_Only(M, fInverse, qSrc, pDst)) return;

	int* based2 = new int[M+1];
	int N, n, half_n, i, j, o, z;
	CComplexNumber* pev;
	CComplexNumber* pod;
	CComplexNumber* pw;
	CComplexNumber t1, t2;

	based2[0] = 1; for ( i = 1; i <= M; ++i ) based2[i] = (based2[i-1] << 1);
	N = based2[M];

	int* vPos = new int[N];
	int* pPos;
	vPos[0] = 0;
	for ( i = 0; i < M; ++i )
	{
		pPos = vPos;
		half_n = based2[i];
		for ( j = 0; j < half_n; ++j ) *(pPos++) <<= 1;
		memcpy(pPos, vPos, sizeof(*pPos) * half_n);
		for ( j = 0; j < half_n; ++j ) *(pPos++) |= 1;
	}

	if (fInverse)
	{
		const CComplexNumber* qqq;
		pw = pDst;
		for ( o = 0; o < N; ++o )
		{
			qqq = qSrc + vPos[o];
			pw->rel = qqq->rel / N;
			pw->img = qqq->img / N;
			++pw;
		}
	}
	else
	{
		const CComplexNumber* qqq;
		pw = pDst;
		for ( o = 0; o < N; ++o )
		{
			qqq = qSrc + vPos[o];
			pw->rel = qqq->rel;
			pw->img = qqq->img;
			++pw;
		}
	}

	int* vIsZero = new int[N];
	j = 0;
	for ( i = N-1; i >= 0; --i )
	{
		if (pDst[i].fIsZero())
			vIsZero[i] = 1 + (i<N-1 ? vIsZero[i+1] : 0);
		else
			vIsZero[i] = 0;
	}

	if (vIsZero[0] < N)
	{
		CComplexNumber* vw = new CComplexNumber[N];

		if (fInverse)
		{
			pev = vw;
			for ( o = 0; o < N; ++o )
				set_complex_unit(M, o, *(pev++));
		}
		else
		{
			pev = vw;
			*(pev++) = 1.0;
			for ( o = N-1; o > 0; --o )
				set_complex_unit(M, o, *(pev++));
		}

		//t1.rel = (fInverse ? +1 : -1) * (PI+PI) / N;
		//t1.Init_by_Polar(t1.rel); t2 = 1.0;
		//for ( o = 0; o < N; ++o ) { vw[o] = t2; t2 *= t1; }

		for ( i = 0; i < M; ++i )
		{
			half_n = based2[i]; n = half_n + half_n; z = based2[M-1-i];
			for ( j = 0; j < N; j += n )
			{
				o = vIsZero[j];
				if (o >= n)
				{
					j += ((o-n) & ~(n-1));
					continue;
				}

				pev = pod = pDst + j; pod += half_n; pw = vw;
				for ( o = 0; o < half_n; ++o, ++pev, ++pod, pw+=z )
				{
					//if (pod->fIsZero())
					//{
					//	pod->rel = pev->rel;
					//	pod->img = pev->img;
					//	continue;
					//}
					t1.rel = pev->rel;
					t1.img = pev->img;
					t2.rel = pod->rel * pw->rel - pod->img * pw->img;
					t2.img = pod->img * pw->rel + pod->rel * pw->img;
					pev->rel += t2.rel;
					pev->img += t2.img;
					pod->rel = t1.rel - t2.rel;
					pod->img = t1.img - t2.img;
					//t1 = *pev;
					//t2 = *pod * vw[z * o];
					//*pev += t2;
					//*pod = t1 - t2;
				}
			}
		}
		delete[] vw;
	}

	delete[] vIsZero;
	delete[] vPos;
	delete[] based2;
}

void loirey::fft::fft_SplitRadix(
	int M, bool fInverse,
	const CComplexNumber* qSrc,
	CComplexNumber* pDst )
{
	if (fft_Small_M_Only(M, fInverse, qSrc, pDst)) return;

	//if (M <= 3)
	//{
	//	fft_Radix2(M, fInverse, qSrc, pDst);
	//	return;
	//}

	int N, N2, N4, i, iii;
	N = (1 << M);
	N2 = N >> 1;
	N4 = N >> 2;

	CComplexNumber* vTemp = new CComplexNumber[N];
	CComplexNumber* pY0;
	CComplexNumber* pY2;
	CComplexNumber* pY1;
	CComplexNumber* pY3;
	const CComplexNumber* qX;
	const CComplexNumber* qX0;
	const CComplexNumber* qX2;
	const CComplexNumber* qX1;
	const CComplexNumber* qX3;

	qX = qSrc;
	pY0 = pDst;
	pY2 = pY0 + N4;
	pY1 = pY2 + N4;
	pY3 = pY1 + N4;
	for ( i = 0; i < N4; ++i )
	{
		(pY0++)->myInit(qX++);
		(pY1++)->myInit(qX++);
		(pY0++)->myInit(qX++);
		(pY3++)->myInit(qX++);
	}

	qX0 = pDst;
	qX2 = qX0 + N4;
	qX1 = qX2 + N4;
	qX3 = qX1 + N4;
	pY0 = vTemp;
	pY2 = pY0 + N4;
	pY1 = pY2 + N4;
	pY3 = pY1 + N4;
	fft_SplitRadix(M-1, fInverse, qX0, pY0);
	fft_SplitRadix(M-2, fInverse, qX1, pY1);
	fft_SplitRadix(M-2, fInverse, qX3, pY3);

	CComplexNumber t1, t3, s1, s3;
	if (fInverse)
	{
		for ( i = 0; i < N4; ++i, ++pY0, ++pY2, ++pY1, ++pY3 )
		{
			pY0->rel *= 0.5; pY0->img *= 0.5;
			pY2->rel *= 0.5; pY2->img *= 0.5;
			pY1->rel *= .25; pY1->img *= .25;
			pY3->rel *= .25; pY3->img *= .25;
		}
		pY0 = vTemp;
		pY2 = pY0 + N4;
		pY1 = pY2 + N4;
		pY3 = pY1 + N4;
	}
	if (!fInverse)
	{
		for ( i = 0, iii = 0; i < N4; ++i, iii+=3, ++pY1, ++pY3 )
		{
			set_complex_unit(M, i, t1);
			set_complex_unit(M, iii, t3);
			s1.rel = pY1->rel * t1.rel + pY1->img * t1.img;
			s1.img = pY1->img * t1.rel - pY1->rel * t1.img;
			s3.rel = pY3->rel * t3.rel + pY3->img * t3.img;
			s3.img = pY3->img * t3.rel - pY3->rel * t3.img;
			pY1->rel = s1.rel + s3.rel;
			pY1->img = s1.img + s3.img;
			pY3->rel = s3.img - s1.img;
			pY3->img = s1.rel - s3.rel;
		}
	}
	else
	{
		//s3.rel = pY3->rel;
		//s3.img = pY3->img;
		//pY3->rel = pY1->img - s3.img;
		//pY3->img = s3.rel - pY1->rel;
		//pY1->rel += s3.rel;
		//pY1->img += s3.img;
		//++pY1; ++pY3;
		//for ( i = 1, iii = 3; i < N4; ++i, iii+=3, ++pY1, ++pY3 )
		for ( i = 0, iii = 0; i < N4; ++i, iii+=3, ++pY1, ++pY3 )
		{
			set_complex_unit(M, i, t1);
			set_complex_unit(M, iii, t3);
			s1.rel = pY1->rel * t1.rel - pY1->img * t1.img;
			s1.img = pY1->img * t1.rel + pY1->rel * t1.img;
			s3.rel = pY3->rel * t3.rel - pY3->img * t3.img;
			s3.img = pY3->img * t3.rel + pY3->rel * t3.img;
			pY1->rel = s1.rel + s3.rel;
			pY1->img = s1.img + s3.img;
			pY3->rel = s1.img - s3.img;
			pY3->img = s3.rel - s1.rel;
		}
	}

	qX0 = vTemp;
	qX2 = qX0 + N4;
	qX1 = qX2 + N4;
	qX3 = qX1 + N4;
	pY0 = pDst;
	pY1 = pY0 + N4;
	pY2 = pY1 + N4;
	pY3 = pY2 + N4;
	for ( i = 0; i < N4; ++i, ++qX0, ++qX1, ++qX2, ++qX3, ++pY0, ++pY1, ++pY2, ++pY3 )
	{
		pY0->rel = qX0->rel + qX1->rel;
		pY0->img = qX0->img + qX1->img;
		pY2->rel = qX0->rel - qX1->rel;
		pY2->img = qX0->img - qX1->img;
		pY1->rel = qX2->rel - qX3->rel;
		pY1->img = qX2->img - qX3->img;
		pY3->rel = qX2->rel + qX3->rel;
		pY3->img = qX2->img + qX3->img;
	}

	delete[] vTemp;
}

void loirey::fft::basic_perform_fft(
	int M, bool fInverse,
	const CComplexNumber* qSrc,
	CComplexNumber* pDst )
{
	if (fft_Small_M_Only(M, fInverse, qSrc, pDst)) return;

	ensure_unit_roots(M);

	fft_Radix2(M, fInverse, qSrc, pDst);
	//fft_SplitRadix(M, fInverse, qSrc, pDst);
}

bool loirey::fft::Perform_FFT_1D(
	const CComplexNumber* qData, int Size, int Step,
	int HeadIndex, int TailIndex, int BorderMode,
	int CenterIndex, bool fInverseData,
	CComplexNumber* pResult, int M, int N,
	bool fInverseFFT)
{
	bool ret;

	if (Step==1 && !fInverseFFT &&
		HeadIndex==CenterIndex && CenterIndex+N==TailIndex &&
		0<=CenterIndex && CenterIndex+N<=Size)
	{
		basic_perform_fft(M, fInverseFFT, qData+CenterIndex, pResult);
		ret = true;
	}
	else
	{
		CSimpleTypeArray<CComplexNumber> vData;
		vData.Resize(N);
		ret = basic_format_data_1D(
			qData, Size, Step,
			HeadIndex, TailIndex, BorderMode,
			CenterIndex, fInverseData,
			vData.pElement, M, N);
		if (ret)
			basic_perform_fft(M, fInverseFFT, vData.pElement, pResult);
	}

	return ret;
}

double loirey::fft::CostConvolution_1D(
	int ResultHeadIndex, int ResultTailIndex,
	bool fExpandDataHead, bool fExpandDataTail,
	int MaskLength, int MaskCenterIndex )
{
	double ret = COST_COEFFICIENT_Convolution;
	int M, N, i;
	i = ResultTailIndex - ResultHeadIndex;
	ret /= i * MaskLength;
	if (fExpandDataHead) i += MaskCenterIndex;
	if (fExpandDataTail) i += MaskLength-1-MaskCenterIndex;
	for ( M=0,N=1; N<i; ++M ) N <<= 1;
	ret *= (1.0 + M) * N;
	return ret;
}

double loirey::fft::CostMultiplyPolynomial(
	int LengthA, int LengthB )
{
	double ret = COST_COEFFICIENT_MultiplyPolynomial;
	int M, N, i;
	ret /= LengthA * LengthB;
	i = LengthA + LengthB - 1;
	for ( M=0,N=1; N<i; ++M ) N <<= 1;
	ret *= (1.0 + M) * N;
	return ret;
}

double loirey::fft::CostConvolution_2D(
	int Result_xHead, int Result_yHead, int Result_xTail, int Result_yTail,
	bool fExpand_xHead, bool fExpand_yHead, bool fExpand_xTail, bool fExpand_yTail,
	int MaskW, int MaskH,
	int Mask_xCenter, int Mask_yCenter )
{
	double ret = COST_COEFFICIENT_Convolution;
	int MW, MH, NW, NH, iW, iH;
	iW = Result_xTail - Result_xHead;
	iH = Result_yTail - Result_yHead;
	ret /= iW * iH * MaskW * MaskH;

	if (fExpand_xHead) iW += Mask_xCenter;
	if (fExpand_yHead) iH += Mask_yCenter;
	if (fExpand_xTail) iW += MaskW-1-Mask_xCenter;
	if (fExpand_yTail) iH += MaskH-1-Mask_yCenter;

	for ( MW=0,NW=1; NW<iW; ++MW ) NW <<= 1;
	for ( MH=0,NH=1; NH<iH; ++MH ) NH <<= 1;
	ret *= (2.0 + MW + MH) * NW * NH;
	return ret;
}

